#include<stdio.h>

#include <Algorithm/points_cloud_algorithm.h>



int main()
{
    int len=4;
    double **data=(double**)malloc(sizeof(double*)*len );
    for(int i=0;i<len;i++)
    {
        data[i]=(double*)malloc(sizeof(double)*3);
        memset(data[i],0,sizeof(double)*3);
    }
    data[0][0]=0;data[0][1]=0;data[0][2]=0;
    data[1][0]=1;data[1][1]=0;data[1][2]=0;
    data[2][0]=0;data[2][1]=1;data[2][2]=0;
    data[3][0]=0;data[3][1]=0;data[3][2]=1; 
  
    double **data1=(double**)malloc(sizeof(double*)*len );
    for(int i=0;i<len;i++)
    {
        data1[i]=(double*)malloc(sizeof(double)*3);
        memset(data1[i],0,sizeof(double)*3);
    }
    
    data1[0][0]=0;data1[0][1]=0;data1[0][2]=0;
    data1[1][0]=0;data1[1][1]=1;data1[1][2]=0;
    data1[2][0]=-1;data1[2][1]=0;data1[2][2]=0;
    data1[3][0]=0;data1[3][1]=0;data1[3][2]=1; 
    






    double** trans=compute_optimal_rigid_body_transformation(data,data1,len);

    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            printf("%lf ",trans[i][j]);
        }
        printf("\n");
    } 


    printf("end\n");
    
    return 0;
}